/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"
#include "ownutils.h"
#include "CParticleFilter.h"

#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

#define PLOT_ENABLED

#define DEFAULT_J2B2_MEAS_FILE "../../../data/j2b2_measurement.txt"
#define DEFAULT_MAP_FILE "../../res/autmap-strip.txt"

void timer(bool isStart){
		static long int ss1,us1;
		long int ss2,us2,sec,us,ms;
		float fms;
		
		if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
		else{
			ownTime_GetTimeOfDay(&ss2,&us2);
			sec = ss2-ss1;
			us = us2-us1;
			ms = sec*1000+us/1000;
			fms=(float)sec*1000.0+(float)us/1000.0;
			fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		}
}

/// For reading J2B2 data from filet
int readScanLine181(FILE *f,Grid::TScan &meas, float &x,float &y,float &a){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&x,&y,&a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						//fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, mcl::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}


void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
						if(v>0.6) v = 0; else v = 0.99; 
				
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpixBold(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpix(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
							}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float angle = M_PI/4;
		for(int i=0;i<3;i++){
				drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/2;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/** 
* Sliding window = Record N scans and fit them together to the map
* And use M old scans for matching as well 
* @param *scan_meas odometry/scan input file name (currently J2B2)  
* @param *env_out_file CEnvironmentMeasurement2D output file
* @param *scan_meas_out outputs the scan/odo measurement file
* @param visu set true to use visualization
**/
void SlidingWindowBackLog(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
	CEnvironmentMeasurement2D meas;	
	float grid_size = 80.0;
	float maxLaserDistance = 40.0;
	int param_NumScans = 2;
	static int NumScans =  param_NumScans;
	float param_sx = 0.3;
	float param_sy = 0.3;
	float param_sa = 30.0*M_PI/180.0;
	static int OldScans = 0;
	 
	
	CBetBdtLocalizer slammer(grid_size,0.06);

#ifdef PLOT_ENABLED
	int Nx,Ny;	
	unsigned char *rgb;
	slammer.grid->getDim(Nx,Ny);
	rgb = (unsigned char *) malloc(3*Nx*Ny);
	int jpgcnt=0;
	addImageWindow(Nx, Ny);
	glutImageGUI_Start();
#endif
	
	
	TOccupancyGrid O(2.0*maxLaserDistance+4.0,2.0*maxLaserDistance+4.0,0.05,Grid::TCoord2D(0,0));
	O.setGridToValue(0.5);
	
	float x=0,y=0,a=0;
	int cnt = 0;
	Grid::TScan scan;
	scan.alloc(600);
	
	//COccuGUI sgui(grid_size,0.06,1.0); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
	FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("j2b2_measurement_labrat.txt","rt");
	if(fmeas == NULL){
		fprintf(stderr,"Unable to open the measurement file '%s'!!!\n",scan_meas);
		exit(1);
	}
	
	FILE *fenv_meas = fopen(env_out_file,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	FILE *fmeas_out = fopen(scan_meas_out,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	
	float search_x = param_sx;
	float search_y = param_sy;
	float search_a = param_sa;
	
	static float L1 = 1.750; ///Offset between laser and the front axis
	static float LaserOffset = -M_PI; ///The heading of laser with respect to odometry axis
	
	CMatchStorage matches(100);
	bool run = true;
	
	mcl::pose init_est,diff,pos;
	mcl::pose last,current;
	
	mcl::pose reference_pose, pose_now;
	
	std::vector< std::vector<float> > scans; ///All scans for previous map
	std::vector< mcl::pose > poses;    ///All poses for previous map
	std::vector<float> tmp_scan;
	
	std::vector< mcl::pose > all_poses;    ///All poses for previous map
	
	///Read first measurement and put it into the map
	//for(int i = 0;i<200;i++) 
	readScanLine(fmeas,scan,last);
	
	///Move to laser frame
	last.x = last.x - L1 * cos(last.a);
	last.y = last.y - L1 * sin(last.a);
	//last.set(x,y,a);
	
	slammer.setMeasurementToMap(scan, 0, 0,  LaserOffset, 0.99);
	writeScanLine(fmeas_out,scan ,0.f,0.f,0.f);
	
	fprintf(stderr,"Scan has %d points\n",scan.N);
	for(int i=0;i<scan.N;i++){
		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
	}
	fprintf(stderr,"\n\n");
	slammer.tuneParameters(15.0,0.6);
	bool updated = false;
	
	std::vector<mcl::pose> pbuf;
	std::vector< std::vector<float> > sbuf; ///All scans for previous map
	FILE *logset = fopen("0_local-map-stats.txt","wt");
	float mapLikelihood=0;
	int scansInMap=0;
	while(run){
		
		///Read NumScans scans from file and set them to Buf.
		for(int i = 0;i<NumScans;i++){
			readScanLine(fmeas,scan,current);
			if(feof(fmeas)){
				///Quit
				run = false;
				break;
			}
			current.x = current.x - L1 * cos(current.a);		
			current.y = current.y - L1 * sin(current.a);
			///Save scans and poses 
			tmp_scan.clear();
			for(int i=0;i<scan.N;i++){
				tmp_scan.push_back(scan.r[i]);
			}
			scans.push_back(tmp_scan);
			
			
			diff.setToDifferentialPose(current,last);
			init_est = init_est.integrateDifferential(diff);
			
			pbuf.push_back(init_est);
			sbuf.push_back(tmp_scan);
			last=current;
		}
		///Now set these scans to an Occupancy Grid Starting from the last one
		/// Init_est is the expected pose
		O.setGridToValue(0.5);
		O.setScanFillProb(0, 0 ,LaserOffset,scan,0.3,0.95); ///Set the latest measurement to Origin
		
		for(unsigned int i=1;i<sbuf.size();i+=1){
			if(sbuf.size()-i-1<0) continue;
			for(int j=0;j<scan.N;j++){
				scan.r[j]=sbuf[sbuf.size()-i-1][j]; ///< starting from the last measurement
				if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
			}
			diff.setToDifferentialPose(pbuf[sbuf.size()-1-i],pbuf[sbuf.size()-1]);
			O.setScanFillProb(diff.x,diff.y,diff.a+LaserOffset,scan,0.3,0.95); ///Set the Rest
		}
		
		meas.points.clear();
		meas.setOccupancyGrid(&O, 0.8);
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		meas.frame.setError(search_x,search_y,search_a);
		
		timer(true);
		int numofnodes=0;
		if(slammer.localizeExt(meas,matches,numofnodes,5000)){
			search_x = param_sx;
			search_y = param_sy;
			search_a = param_sa;
			NumScans = param_NumScans;

			matches.sort();
			
			TCovariance c;
			slammer.computeCovariance(matches.storage[0], meas, 15,15,15,0.02,0.02, 0.1*M_PI/180.0,c);
			
			fprintf(stderr,"Res:: (%.2f,%.2f,%.2f) w1=%.4f M:(G:%d B:%d Fit:%.3f)\n avg:(%lf,%lf,%lf) var:(%lf,%lf,%lf) xy=%lf xa=%lf ya =%lf\n",
							matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,
							matches.storage[0].weight,
							matches.storage[0].StatNumGoodMeas, 
							matches.storage[0].StatNumOutliers, 
							matches.storage[0].StatMeasurementDistSum,
							c.ax,c.ay,c.aa,
							c.cxx,c.cyy,c.caa,
							c.cxy, c.cxa, c.cya);
							fprintf(logset,"Res:: (%.2f,%.2f,%.2f) w1=%.4f M:(G:%d B:%d Fit:%.3f) avg:(%lf,%lf,%lf) var:(%lf,%lf,%lf) xy=%lf xa=%lf ya =%lf\n",
							matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,
							matches.storage[0].weight,
							matches.storage[0].StatNumGoodMeas, 
							matches.storage[0].StatNumOutliers, 
							matches.storage[0].StatMeasurementDistSum,
							c.ax,c.ay,c.aa,
							c.cxx,c.cyy,c.caa,
							c.cxy, c.cxa, c.cya);
			mapLikelihood += matches.storage[0].StatMeasurementDistSum / matches.storage[0].StatNumGoodMeas;
			scansInMap++;
			/****************************
			for(int i=1;i<matches.storage.size();i++){
				float distToCorrect = (matches.storage[0].x-matches.storage[i].x)*(matches.storage[0].x-matches.storage[i].x) +
				(matches.storage[0].y-matches.storage[i].y)*(matches.storage[0].y-matches.storage[i].y);
				float headdif = fabs(matches.storage[0].a-matches.storage[i].a);
				if(distToCorrect > 0.05*0.05 || headdif > 1.0*M_PI/180.0){
					if(!matches.storage[i].isInside(matches.storage[0])){
						
						fprintf(stderr,"Dual '%d' e_d=%.3f e_a=%.3f, w1=%.4f, w2=%.4f (%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f) M:(G:%d B:%d Fit:%.3f) D:(G:%d B:%d Fit:%.3f)\n",
									i,
									sqrt(distToCorrect),
									180.0*headdif/M_PI,
									sqrt(matches.storage[0].weight/180.0), 
									sqrt(matches.storage[i].weight/180.0),
									matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,
									matches.storage[i].x,matches.storage[i].y,matches.storage[i].a,
										matches.storage[0].StatNumGoodMeas, 
										matches.storage[0].StatNumOutliers, 
										matches.storage[0].StatMeasurementDistSum, 
										matches.storage[i].StatNumGoodMeas, 
										matches.storage[i].StatNumOutliers, 
										matches.storage[i].StatMeasurementDistSum 
									);
					}else{
						fprintf(stderr,"Its IN!\n");
					}
				}
			}
			***********************************/
			
			meas.frame.x_ref = matches.storage[0].x; meas.frame.y_ref = matches.storage[0].y; meas.frame.a_ref = matches.storage[0].a;
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			//writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"Nodes=%d, Pose now (%.2f %.2f %.2f) \n",numofnodes, pose_now.x,pose_now.y,pose_now.a );
			
			///Now we should compute the poses backwards 
			for(int i=OldScans;i<pbuf.size();i++){
				
				//diff.setToDifferentialPose(pbuf[pbuf.size()-1],pbuf[i]); ///Difference from current to reference
				diff.setToDifferentialPose(pbuf[i],pbuf[pbuf.size()-1]); ///Difference from current to reference
				
				
				mcl::pose ptmp = init_est;
				ptmp.integrateDifferential(diff);
				poses.push_back(ptmp);
				all_poses.push_back(ptmp);
				for(int j=0;j<scan.N;j++){
					scan.r[j]=sbuf[i][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				
				slammer.setMeasurementToMap(scan,ptmp.x,ptmp.y,ptmp.a+LaserOffset,0.99,false);
				
				ptmp = pose_now.integrateDifferential(diff);
				/*
				fprintf(stderr,"Last:(%.2f, %.2f, %.2f), ToComp:(%.2f, %.2f, %.2f), Diff:(%.2f, %.2f, %.2f), pose:(%.2f, %.2f, %.2f)\n",
								pbuf[pbuf.size()-1].x, pbuf[pbuf.size()-1].y, pbuf[pbuf.size()-1].a,
								pbuf[i].x, pbuf[i].y, pbuf[i].a,
								diff.x, diff.y, diff.a,
								ptmp.x, ptmp.y, ptmp.a);
			*/
			
			writeScanLine(fmeas_out,scan, ptmp.x,ptmp.y,ptmp.a);
								
								
			}
			while(pbuf.size()>OldScans){
				pbuf.pop_back();
				sbuf.pop_back();
			}
#ifdef PLOT_ENABLED
			slammer.grid->fillRGB(rgb,1.0/255.0);
			for(int i=0;i<all_poses.size();i++){
				Grid::TPixel pix = slammer.grid->toPixel(Grid::TCoord2D(all_poses[i].x,all_poses[i].y));
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
			}
			
			for(int i=0;i<meas.size();i++){
				//float xc = cos(pose_now.a)*meas.points[i].x - sin(pose_now.a) * meas.points[i].y + pose_now.x;
				//float yc = sin(pose_now.a)*meas.points[i].x + cos(pose_now.a) * meas.points[i].y + pose_now.y;
				
				float xc = cos(init_est.a)*meas.points[i].x - sin(init_est.a) * meas.points[i].y + init_est.x;
				float yc = sin(init_est.a)*meas.points[i].x + cos(init_est.a) * meas.points[i].y + init_est.y;
				
				Grid::TPixel pix = slammer.grid->toPixel(Grid::TCoord2D(xc,yc));
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255,255 );
			}
			displayImage(0, rgb,Ny,Nx);
#endif
			slammer.updateDistTransform();
		}else{
			search_x += .05;
			search_y += .05;
			search_a += 2.0*M_PI/180.0;
			if(NumScans < 3) NumScans++;
			
			fprintf(stderr,"Loclize Failed!\n");
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			//writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"nodes=%d, Pose now (%.2f %.2f %.2f) ",numofnodes, pose_now.x,pose_now.y,pose_now.a );
			
			///Now we should compute the poses backwards 
			for(int i=OldScans;i<pbuf.size();i++){
				
				//diff.setToDifferentialPose(pbuf[pbuf.size()-1],pbuf[i]); ///Difference from current to reference
				diff.setToDifferentialPose(pbuf[i],pbuf[pbuf.size()-1]); ///Difference from current to reference
				
				
				mcl::pose ptmp = init_est;
				ptmp.integrateDifferential(diff);
				poses.push_back(ptmp);
				
				for(int j=0;j<scan.N;j++){
					scan.r[j]=0;
					//scan.r[j]=sbuf[i][j]; ///< starting from the last measurement
					//if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				
				//slammer.setMeasurementToMap(scan,ptmp.x,ptmp.y,ptmp.a+LaserOffset,0.97,false);
				
				ptmp = pose_now.integrateDifferential(diff);
				
				fprintf(stderr,"Last:(%.2f, %.2f, %.2f), ToComp:(%.2f, %.2f, %.2f), Diff:(%.2f, %.2f, %.2f), pose:(%.2f, %.2f, %.2f)\n",
								pbuf[pbuf.size()-1].x, pbuf[pbuf.size()-1].y, pbuf[pbuf.size()-1].a,
								pbuf[i].x, pbuf[i].y, pbuf[i].a,
								diff.x, diff.y, diff.a,
								ptmp.x, ptmp.y, ptmp.a);
								writeScanLine(fmeas_out,scan, ptmp.x,ptmp.y,ptmp.a);
								
								
			}
			while(pbuf.size()>OldScans){
				pbuf.pop_back();
				sbuf.pop_back();
			}
			slammer.updateDistTransform();
		}
		
		///Compute if we are at new or old place
		float value_sum=0;
		int num =0;
		for(int i=1;i<10;i++){
			float xxxx = pose_now.x + i * 0.05 * cos(pose_now.a);
			float yyyy = pose_now.y + i * 0.05 * sin(pose_now.a);
			
			float value;
			if(slammer.grid->getValue(Grid::TCoord2D(xxxx,yyyy), value)){
				value_sum += value;
				num++;
			}
		}
		if(num>0){
			value_sum/=num;
			if( fabs(value_sum-0.5)<0.05){
				fprintf(stderr,"Avg=%.2f Most Likely we are exploring new terretories\n",value_sum);
			}else{
				fprintf(stderr,"Avg=%.2f On the road again!\n",value_sum);
			}
		}
		
		
		
		
		
		////////////////////////////////////////////////////////////////////////////
		/// Resetting the map
		////////////////////////////////////////////////////////////////////////////
		pos = init_est;
		float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
		float toEdge;
		toEdge = (grid_size/2.0)-toPos;			
		if(toEdge < 30.0 || feof(fmeas)){
			char fname[60];
			sprintf(fname,"%d_localmap.jpg",jpgcnt);
			saveJPEG(fname, rgb,Nx, Ny);
			fprintf(logset,"Map likelihood = %.4f Scans= %d \n",mapLikelihood/scansInMap, scansInMap);
			
			scansInMap=0;
			mapLikelihood=0;
			
			fclose(logset);
			
			sprintf(fname,"%d_local-map-stats.txt",jpgcnt);
			logset = fopen(fname,"wt");
			jpgcnt++;
			//meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
			//meas.frame.setFrame(pos.x,pos.y,pos.a);		
			//meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
			
			//fprintf(stderr,"Saving Original map\n");
			//meas.fprint(fenv_meas);        ///< Save to file
			//fflush(fenv_meas);             ///< flush
			slammer.grid->setGridToValue(0.5); ///< Clear map
			
			reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
			pos.set(0,0,0); ///< initialize pose
			init_est.set(0,0,0);
			
			all_poses.clear();
			
			for(unsigned int i=1;i<scans.size()-2;i+=1){
				for(int j=0;j<scan.N;j++){
					scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
				slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a+LaserOffset,0.99,false);
			}
			slammer.updateDistTransform();
			
			// 			if(visu){
				// 				sgui.setOccupancyGrid(slammer.grid);
				// 				//sgui.save();
				// 				updated=true;
				// 			}
				scans.clear();
				poses.clear();
		}
		timer(false);
		cnt++;	
	}
}




///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/** 
* Sliding window = Record N scans and fit them together to the map
*
* @param *scan_meas odometry/scan input file name (currently J2B2)  
* @param *env_out_file CEnvironmentMeasurement2D output file
* @param *scan_meas_out outputs the scan/odo measurement file
* @param visu set true to use visualization
**/
void SlidingWindow(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
	CEnvironmentMeasurement2D meas;	
	float grid_size = 150.0;
	float maxLaserDistance = 60.0;
	static int NumScans = 150;

	CBetBdtLocalizer slammer(grid_size,0.05);
	
	TOccupancyGrid O(2.0*maxLaserDistance+4.0,2.0*maxLaserDistance+4.0,0.05,Grid::TCoord2D(0,0));
	O.setGridToValue(0.5);
	
	float x=0,y=0,a=0;
	int cnt = 0;
	Grid::TScan scan;
	scan.alloc(600);
	
	//COccuGUI sgui(grid_size,0.06,1.0); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
	FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("j2b2_measurement_labrat.txt","rt");
	if(fmeas == NULL){
		fprintf(stderr,"Unable to open the measurement file '%s'!!!\n",scan_meas);
		exit(1);
	}
	
	FILE *fenv_meas = fopen(env_out_file,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	FILE *fmeas_out = fopen(scan_meas_out,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	float search_x = .8;
	float search_y = .8;
	float search_a = 40.0*M_PI/180.0;
	
	static float L1 = 1.750; ///Offset between laser and the front axis
	static float LaserOffset = -M_PI; ///The heading of laser with respect to odometry axis
	
	CMatchStorage matches(100);
	bool run = true;
	
	mcl::pose init_est,diff,pos;
	mcl::pose last,current;
	
	mcl::pose reference_pose, pose_now;
	
	std::vector< std::vector<float> > scans; ///All scans for previous map
	std::vector< mcl::pose > poses;    ///All poses for previous map
	std::vector<float> tmp_scan;
	
	///Read first measurement and put it into the map
	//for(int i = 0;i<200;i++) 
	readScanLine(fmeas,scan,last);
	
	///Move to laser frame
	last.x = last.x - L1 * cos(last.a);
	last.y = last.y - L1 * sin(last.a);
	//last.set(x,y,a);
	
	slammer.setMeasurementToMap(scan, 0, 0,  LaserOffset, 0.99);
	writeScanLine(fmeas_out,scan ,0.f,0.f,0.f);
	
	fprintf(stderr,"Scan has %d points\n",scan.N);
	for(int i=0;i<scan.N;i++){
		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
	}
	fprintf(stderr,"\n\n");
	slammer.tuneParameters(15.0,0.12);
	bool updated = false;
	
	std::vector<mcl::pose> pbuf;
	std::vector< std::vector<float> > sbuf; ///All scans for previous map
	
	while(run){
		pbuf.clear();
		sbuf.clear();
		///Read NumScans scans from file and set them to Buf.
		for(int i = 0;i<NumScans;i++){
			readScanLine(fmeas,scan,current);
			if(feof(fmeas)){
				///Quit
				run = false;
				break;
			}
			current.x = current.x - L1 * cos(current.a);		
			current.y = current.y - L1 * sin(current.a);
			///Save scans and poses 
			tmp_scan.clear();
			for(int i=0;i<scan.N;i++){
				tmp_scan.push_back(scan.r[i]);
			}
			scans.push_back(tmp_scan);
			
			
			diff.setToDifferentialPose(current,last);
			init_est = init_est.integrateDifferential(diff);
			
			pbuf.push_back(init_est);
			sbuf.push_back(tmp_scan);
			last=current;
		}
		///Now set these scans to an Occupancy Grid Starting from the last one
		/// Init_est is the expected pose
		O.setGridToValue(0.5);
		O.setScanFillProb(0, 0 ,LaserOffset,scan,0.3,0.95); ///Set the latest measurement to Origin
		
		for(unsigned int i=1;i<sbuf.size();i+=1){
			if(sbuf.size()-i-1<0) continue;
			for(int j=0;j<scan.N;j++){
				scan.r[j]=sbuf[sbuf.size()-i-1][j]; ///< starting from the last measurement
				if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
			}
			diff.setToDifferentialPose(pbuf[sbuf.size()-1-i],pbuf[sbuf.size()-1]);
			O.setScanFillProb(diff.x,diff.y,diff.a+LaserOffset,scan,0.3,0.95); ///Set the Rest
		}
		
		meas.points.clear();
		meas.setOccupancyGrid(&O, 0.8);
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		meas.frame.setError(search_x,search_y,search_a);
		
		timer(true);
		
		if(slammer.localize(meas,matches)){
			matches.sort();
			meas.frame.x_ref = matches.storage[0].x; meas.frame.y_ref = matches.storage[0].y; meas.frame.a_ref = matches.storage[0].a;
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			//writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"Pose now (%.2f %.2f %.2f) ",pose_now.x,pose_now.y,pose_now.a );
			
			///Now we should compute the poses backwards 
			for(int i=0;i<pbuf.size();i++){
				
				//diff.setToDifferentialPose(pbuf[pbuf.size()-1],pbuf[i]); ///Difference from current to reference
				diff.setToDifferentialPose(pbuf[i],pbuf[pbuf.size()-1]); ///Difference from current to reference
				
				
				mcl::pose ptmp = init_est;
				ptmp.integrateDifferential(diff);
				poses.push_back(ptmp);
				
				for(int j=0;j<scan.N;j++){
					scan.r[j]=sbuf[i][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				
				slammer.setMeasurementToMap(scan,ptmp.x,ptmp.y,ptmp.a+LaserOffset,0.97,false);
				
				ptmp = pose_now.integrateDifferential(diff);
				
				fprintf(stderr,"Last:(%.2f, %.2f, %.2f), ToComp:(%.2f, %.2f, %.2f), Diff:(%.2f, %.2f, %.2f), pose:(%.2f, %.2f, %.2f)\n",
								pbuf[pbuf.size()-1].x, pbuf[pbuf.size()-1].y, pbuf[pbuf.size()-1].a,
								pbuf[i].x, pbuf[i].y, pbuf[i].a,
								diff.x, diff.y, diff.a,
								ptmp.x, ptmp.y, ptmp.a);
				writeScanLine(fmeas_out,scan, ptmp.x,ptmp.y,ptmp.a);
				
				
			}
			slammer.updateDistTransform();
		}else{
			fprintf(stderr,"WTF!!!!\n");
		}
		
		
		////////////////////////////////////////////////////////////////////////////
		/// Resetting the map
		////////////////////////////////////////////////////////////////////////////
		pos = init_est;
		float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
		float toEdge;
		toEdge = (grid_size/2.0)-toPos;			
		if(toEdge < 30.0 || feof(fmeas)){
			meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
			meas.frame.setFrame(pos.x,pos.y,pos.a);		
			meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
			
			fprintf(stderr,"Saving Original map\n");
			meas.fprint(fenv_meas);        ///< Save to file
			fflush(fenv_meas);             ///< flush
			//slammer.grid->setGridToValue(0.5); ///< Clear map
	
	reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
	pos.set(0,0,0); ///< initialize pose
	init_est.set(0,0,0);
	
	for(unsigned int i=1;i<scans.size()-2;i+=1){
		for(int j=0;j<scan.N;j++){
			scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
			if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
		}
		diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
		slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a+LaserOffset,0.99,false);
	}
	slammer.updateDistTransform();
	
	// 			if(visu){
		// 				sgui.setOccupancyGrid(slammer.grid);
		// 				//sgui.save();
		// 				updated=true;
		// 			}
		scans.clear();
		poses.clear();
		}
		timer(false);
		cnt++;	
	}
}





///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/** 
* Test Very Local SLAM = Scan Match
* A "real-time" version of the B&B 
* Localizes on a small local map.
*
* @param *scan_meas odometry/scan input file name (currently J2B2)  
* @param *env_out_file CEnvironmentMeasurement2D output file
* @param *scan_meas_out outputs the scan/odo measurement file
* @param visu set true to use visualization
**/
void ScanMatchDemo(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
	CEnvironmentMeasurement2D meas;	
	float grid_size = 100.0;
	float maxLaserDistance = 45.0;
	CBetBdtLocalizer slammer(grid_size,0.08); 
	float x=0,y=0,a=0;
	int cnt = 0;
	Grid::TScan scan;
	scan.alloc(600);
		
	//COccuGUI sgui(grid_size,0.06,1.0); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
		FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("j2b2_measurement_labrat.txt","rt");
	if(fmeas == NULL){
		fprintf(stderr,"Unable to open the measurement file '%s'!!!\n",scan_meas);
		exit(1);
	}
		
	FILE *fenv_meas = fopen(env_out_file,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
		
	FILE *fmeas_out = fopen(scan_meas_out,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	float search_x = .15;
	float search_y = .15;
	float search_a = 10.0*M_PI/180.0;
	
	static float L1 = 1.750; ///Offset between laser and the front axis
	static float LaserOffset = -M_PI; ///The heading of laser with respect to odometry axis
	
	CMatchStorage matches(100);
	bool run = true;
	
	mcl::pose init_est,diff,pos;
	mcl::pose last,current;
		
	mcl::pose reference_pose, pose_now;
	
	std::vector< std::vector<float> > scans; ///All scans for previous map
	std::vector< mcl::pose > poses;    ///All poses for previous map
	std::vector<float> tmp_scan;
		
		///Read first measurement and put it into the map
	//for(int i = 0;i<200;i++) 
	readScanLine(fmeas,scan,last);
	
	///Move to laser frame
	last.x = last.x - L1 * cos(last.a);
	last.y = last.y - L1 * sin(last.a);
	//last.set(x,y,a);
		
	slammer.setMeasurementToMap(scan, 0, 0,  LaserOffset, 0.99);
	writeScanLine(fmeas_out,scan ,0.f,0.f,0.f);
		
	fprintf(stderr,"Scan has %d points\n",scan.N);
	for(int i=0;i<scan.N;i++){
		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
	}
	fprintf(stderr,"\n\n");
	slammer.tuneParameters(15.0,0.12);
	bool updated = false;
		
	while(run){
		for(int i = 0;i<1;i++) readScanLine(fmeas,scan,current);
		//current.set(x,y,a);
		current.x = current.x - L1 * cos(current.a);		
		current.y = current.y - L1 * sin(current.a);
				///Save scans and poses 
		tmp_scan.clear();
		for(int i=0;i<scan.N;i++){
			tmp_scan.push_back(scan.r[i]);
		}
		scans.push_back(tmp_scan);
				
				
		diff.setToDifferentialPose(current,last);
		
		init_est = init_est.integrateDifferential(diff);
		last=current;
				
		///Put scan to env meas class
		TMeasurementPoint p;
		meas.points.clear();
		for(int i=0;i<scan.N;i++){
			if(scan.r[i]<maxLaserDistance){
				p.x = scan.r[i]*cos(scan.a[i]+LaserOffset);
				p.y = scan.r[i]*sin(scan.a[i]+LaserOffset);
				meas.addPoint(p);
			}else{
				scan.r[i] = 0;
			}
		}
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		meas.frame.setError(search_x,search_y,search_a);
		
		timer(true);
						
		if(slammer.localize(meas,matches)){
			matches.sort();
			meas.frame.x_ref = matches.storage[0].x; meas.frame.y_ref = matches.storage[0].y; meas.frame.a_ref = matches.storage[0].a;
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"Pose now (%.2f %.2f %.2f) ",pose_now.x,pose_now.y,pose_now.a );
			poses.push_back(init_est); ///Save pose for this scan 
						
			slammer.setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a+LaserOffset,0.99);
			//sgui.setOccupancyGrid(slammer.grid);
			/*		
			if(run && updated){
				run = sgui.plotMatchResults(matches,matches.storage[0].x,
																		matches.storage[0].y,matches.storage[0].a);
				updated = false;
			}*/
		}else{
			fprintf(stderr,"WTF!!!!\n");
		}
				
				
		////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
		////////////////////////////////////////////////////////////////////////////
		pos = init_est;
		float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
		float toEdge;
		toEdge = (grid_size/2.0)-toPos;			
		if(toEdge < 40.0 || feof(fmeas)){
			meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
			meas.frame.setFrame(pos.x,pos.y,pos.a);		
			meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
						
			fprintf(stderr,"Saving Original map\n");
			meas.fprint(fenv_meas);        ///< Save to file
			fflush(fenv_meas);             ///< flush
			//slammer.grid->setGridToValue(0.5); ///< Clear map
			
			reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
			pos.set(0,0,0); ///< initialize pose
			init_est.set(0,0,0);
			
			for(unsigned int i=1;i<scans.size()-2;i+=1){
				for(int j=0;j<scan.N;j++){
					scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
				slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a+LaserOffset,0.99,false);
			}
			slammer.updateDistTransform();
			
// 			if(visu){
// 				sgui.setOccupancyGrid(slammer.grid);
// 				//sgui.save();
// 				updated=true;
// 			}
			scans.clear();
			poses.clear();
		}
		timer(false);
		cnt++;	
	}
}





///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void printUsage(){
		fprintf(stderr,"\n--------------------------------------------------------------------\n");
		fprintf(stderr,"Branch and Bound Localizer Demo Application\n");	
		fprintf(stderr,"\nThis application contains different use cases for BetB_dt Localizer\n");
		fprintf(stderr,"The different cases uses 'simulated' data which is requested as user input.\n");
		fprintf(stderr,"\nUsgage ./bbdt [num]\n");
		fprintf(stderr,"[num] a demo number:\n");
		fprintf(stderr,"0: ScanMatch Demo. 	 [scan_meas] [env_out_file] [scan_meas_out]\n");
		fprintf(stderr,"1: SlidingWindow Demo. 	 [scan_meas] [env_out_file] [scan_meas_out]\n");
		fprintf(stderr,"--------------------------------------------------------------------\n\n\n");
}


int main(int argc, char *argv[]){
	  int demo = 0;
		srand(time(NULL));
		const char *scan_meas=DEFAULT_J2B2_MEAS_FILE;
		const char *env_out_file = "environment_measurement2D_out.txt";
		const char *env_file = "environment_measurement2D.txt";
		const char *scan_meas_out = "bbdt_measurement.txt";		
		
		printUsage();
		if(argc < 2){
				printUsage();
				return 0;
		}
		else{
				demo = atoi(argv[1]);
		}
		
		switch(demo){
				case 0:
					if(argc >= 3) scan_meas = argv[2];
					if(argc >= 4) env_out_file = argv[3];
					if(argc >= 5) scan_meas_out = argv[4];
					
					ScanMatchDemo(scan_meas ,env_out_file ,scan_meas_out , false);	
					break;
				case 1:
					if(argc >= 3) scan_meas = argv[2];
					if(argc >= 4) env_out_file = argv[3];
					if(argc >= 5) scan_meas_out = argv[4];
					
					SlidingWindow(scan_meas ,env_out_file ,scan_meas_out , false);	
					break;
				case 2:
					if(argc >= 3) scan_meas = argv[2];
					if(argc >= 4) env_out_file = argv[3];
					if(argc >= 5) scan_meas_out = argv[4];
					
					SlidingWindowBackLog(scan_meas ,env_out_file ,scan_meas_out , false);	
					break;
				default:
						fprintf(stderr,"Invalid input parameter\n"); 
						break;
		}
		
	
	fprintf(stderr,"Exited\n");	
	return 0;
}